| Note: This tutorial assumes that you have completed the previous tutorials: ROS tutorials. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Feeding detections to the world model
Description: This tutorial shows the input message type of the wire stack. It in addition explains how any detector can be adapted to publish its results to the world model in the appropriate format.Tutorial Level: INTERMEDIATE
Next Tutorial: Using the estimated world state
Goal
This tutorial will show how the world model can be fed with evidence.
Approach
Wire expects the evidence to be of message type wire_msgs/WorldEvidence. Converting detections to the appropriate format can be done using problib in at most a few lines of code. A detailed example available in the src folder of the wire_tutorials package shows how this is done.
1 #include <ros/ros.h>
2
3 #include "wire_msgs/WorldEvidence.h"
4 #include "wire_msgs/ObjectEvidence.h"
5
6 #include "problib/conversions.h"
7
8 // Publisher used to send evidence to world model
9 ros::Publisher world_evidence_publisher_;
10
11
12 void addEvidence(wire_msgs::WorldEvidence& world_evidence, double x, double y, double z, const std::string& class_label, const std::string& color) {
13 wire_msgs::ObjectEvidence obj_evidence;
14
15 // Set the continuous position property
16 wire_msgs::Property posProp;
17 posProp.attribute = "position";
18
19 // Set position (x,y,z), set the covariance matrix as 0.005*identity_matrix
20 pbl::PDFtoMsg(pbl::Gaussian(pbl::Vector3(x, y, z), pbl::Matrix3(0.0005, 0.0005, 0.0005)), posProp.pdf);
21 obj_evidence.properties.push_back(posProp);
22
23 // Set the discrete class label property
24 wire_msgs::Property classProp;
25 classProp.attribute = "class_label";
26 pbl::PMF classPMF;
27
28 // Probability of the class label is 0.7
29 classPMF.setProbability(class_label, 0.7);
30 pbl::PDFtoMsg(classPMF, classProp.pdf);
31 obj_evidence.properties.push_back(classProp);
32
33 // Set the discrete color property with a probability of 0.9
34 wire_msgs::Property colorProp;
35 colorProp.attribute = "color";
36 pbl::PMF colorPMF;
37
38 // The probability of the detected color is 0.9
39 colorPMF.setProbability(color, 0.9);
40 pbl::PDFtoMsg(colorPMF, colorProp.pdf);
41 obj_evidence.properties.push_back(colorProp);
42
43 // Add all properties to the array
44 world_evidence.object_evidence.push_back(obj_evidence);
45 }
46
47
48 void generateEvidence() {
49
50 // Create world evidence message
51 wire_msgs::WorldEvidence world_evidence;
52
53 // Set header
54 world_evidence.header.stamp = ros::Time::now();
55 world_evidence.header.frame_id = "/map";
56
57 // Add evidence
58 addEvidence(world_evidence, 2, 2.2, 3, "mug", "red");
59
60 // Publish results
61 world_evidence_publisher_.publish(world_evidence);
62 ROS_INFO("Published world evidence with size %d", world_evidence.object_evidence.size());
63
64 }
65
66 /**
67 * Main
68 */
69 int main(int argc, char **argv) {
70
71 // Initialize ros and create node handle
72 ros::init(argc,argv,"generate_evidence");
73 ros::NodeHandle nh;
74
75 // Publisher
76 world_evidence_publisher_ = nh.advertise<wire_msgs::WorldEvidence>("/world_evidence", 100);
77
78 // Publish with 3 Hz
79 ros::Rate r(3.0);
80
81 while (ros::ok()) {
82 generateEvidence();
83 r.sleep();
84 }
85
86 return 0;
87 }
The code explained
Here the required headers are included. wire_msgs/WorldEvidence messages are sent to wire_core and contain wire_msgs/ObjectEvidence messages.
Problib is used to simplify creating and converting detections.
The global publisher used for publishing the data to the world model in this example node.
This function adds object evidence to the world evidence message. wire_msgs/WorldEvidence is a collection of wire_msgs/ObjectEvidence messages. wire_msgs/ObjectEvidence is a vector of wire_msgs/Property messages, each property represents one measured object attribute. In its current implementation, different properties are estimated independently of each other.
Here the property 'position' is created.
The position property is defined by means of a Gaussian with a mean of (x,y,z) and a covariance matrix that equals 0.0005 times an identity matrix with size three.
21 obj_evidence.properties.push_back(posProp);
The position property is added to the object evidence message.
Now the property 'class_label' is created.
26 pbl::PMF classPMF;
This time a probability mass function is used to represent the evidence.
29 classPMF.setProbability(class_label, 0.7);
The class label given by the user is set with a probability of 0.7. In a real world application this is the result of some object detection algorithm.
33 // Set the discrete color property with a probability of 0.9
34 wire_msgs::Property colorProp;
35 colorProp.attribute = "color";
36 pbl::PMF colorPMF;
37
38 // The probability of the detected color is 0.9
39 colorPMF.setProbability(color, 0.9);
40 pbl::PDFtoMsg(colorPMF, colorProp.pdf);
41 obj_evidence.properties.push_back(colorProp);
The property color is added in the same manner as the class label, this time with a probability of 0.9.
44 world_evidence.object_evidence.push_back(obj_evidence);
The collection of properties for this object is added to the world evidence.
48 void generateEvidence() {
A function that generates evidence.
Create a message of the appropriate type.
Set the header. Detections can be made with respect to any frame, the world model will transform evidence if needed.
Add a detection of a red mug to the world evidence.
Publish the resulting world_evidence message to the world model and inform the user.
World evidence is published over the /world_evidence topic.
Reproducing the result
Make cloned and compiled wire (this includes the example node explained above):
$ git clone https://github.com/tue-robotics/wire.git $ catkin_make
Start a ros core
$ roscore
Now start the node that was explained above:
$ rosrun wire_tutorials generate_evidence
which creates dummy detections. The following output should appear:
[ INFO] [1359389963.780913354]: Published world evidence with size 1 [ INFO] [1359389964.114297651]: Published world evidence with size 1 [ INFO] [1359389964.447664760]: Published world evidence with size 1
Visualizing the wire world model input or output can be done as explained in this tutorial:
$ roslaunch wire_tutorials rviz_wire_kinetic.launch
